/*
 * Kalman.cc
 *
 *  Created on: Oct 13, 2012
 *      Author: HUNG
 */

#include "Kalman.h"
#include <math.h>

Kalman::Kalman()
{

}

Kalman::Kalman(double TimeStep, double ProcessNoiseIntensity)
{
    this->dt = TimeStep;
    this->processNoiseIntensity = ProcessNoiseIntensity;
    //this->measurementNoiseIntensity = MeasurementNoiseIntensity;

    X0 = Matrix(4, 1, 0);    //Matrix Zezo
    X = Matrix(4, 1, 0);    //Matrix Zezo

    P = Matrix(4, 4, 1);    //Matrix Indentity

    A = Matrix(4, 4, 1);
    A.setData(0, 1, dt);
    A.setData(2, 3, dt);

    B = Matrix(4, 1, 0);

    U = Matrix(1, 1, 0);

    Q = Matrix(4, 4, 0);
    Q.setData(0, 0, pow(dt, 3) / 3); Q.setData(0, 1, pow(dt, 2) / 2);
    Q.setData(1, 0, pow(dt, 2) / 2); Q.setData(1, 1, dt);
    Q.setData(2, 2, pow(dt, 3) / 3); Q.setData(2, 3, pow(dt, 2) / 2);
    Q.setData(3, 2, pow(dt, 2) / 2); Q.setData(3, 3, dt);

    Q = Q * this->processNoiseIntensity;

}

Kalman::~Kalman()
{

}

void Kalman::Predict()
{
    X0 = A * X + B * U;
    P0 = A * P * A.transpose() + Q;
}

void Kalman::Update(Matrix Z)
{
    Matrix S = H * P0 * H.transpose() + R;
    Matrix K = P0 * (H.transpose()) * (S.inverse());
    X = X0 + K * (Z - Z_Pre);
    P = P0 - K * S * K.transpose();
}

